/****************************************************************************************************************************
* The Hand behaviour class controls the location of the hand geometry, provides the flying around the world ability			*
* and allows us to drag objects at random around the world.																	*		
*
* 2/2007 - Andrew Cunningham
* wrote generic hand behavior that contains code shared by both hands\
* fixed the issue where the left hand wouldn't drag an object when the user moved
*																															*
* 1/2005 - Ross Smith																										*																															*
* re-written to remove the floating point errors that occured due to the scene graph design. Previously the hand			*
* and the users view flew around the world by adding an offset to the origin of the polhemus transmitter.  The new			*
* structure moves the world around the users hand and head (view) so both the hand and head are always relative to the		*
* polhemus transmitter.																										*
*																															*	
****************************************************************************************************************************/

import javax.media.j3d.*;
import javax.vecmath.*;

public abstract class HandBehaviorGeneric extends Behavior {

	/*Wake up conditions*/
    protected WakeupOnElapsedFrames nextFrame;
    protected WakeupOnCollisionEntry collision;
    protected WakeupOnCollisionExit collisionExit;
    protected WakeupOnCollisionMovement collisionMovement;
    protected WakeupOr wakeupConditions;

    protected boolean[] button = {false, false, false, false, false};     /*Button states*/
    protected Hand hand;												  /*Geometry of the hand*/
	
	/*Sensors*/
    protected Sensor handTracker;				/* Location sensor for the hand */
    protected ButtonDevice buttonDevice;		/* retrieves controller buttons state */

	/*Transformations*/
    protected TransformGroup viewPlatform;		/* Used to store the view transform (where we are looking) */
    protected Transform3D handTransform;		/* Transform matrix for the hand geometry*/
    protected Transform3D viewTransform;		/* The view transformation matrix info*/
    protected TransformGroup worldBranch; 		/* Transform describing the location of the world */

    protected TransformGroup draggingObject;	/* an objects transform group that is being dragged */
    protected Vector3f pickedObjectOffset;		/* Offset added relative to the hand */
    protected Matrix3d pickedObjectRotation;	/* Offset added relative to the rotation of the hand */
    
    protected static Vector3d currentWorldOffset = new Vector3d();	/* current world offset - static variable shared between all hand instances */
    protected Vector3d previousWorldOffset = new Vector3d();		/* last observed world offset - if this is different to currentWorldOffset, the user has moved */
    protected Vector3d movementOffset = new Vector3d();				/* offset added to an object being dragged while flying around the world */

    protected final static float FLYING_SPEED = 0.1f;	/* speed at which the user flys in the virtual world */
    protected final boolean SixDOF = false;				/* When true run world with 6DOF otherwise run ina easier operating mode*/	
    
    
    public HandBehaviorGeneric(Hand hand, Sensor handTracker, ButtonDevice mouse, TransformGroup viewPlatform, 
		TransformGroup worldBranch) {
    	
    	/*Assign class parameters*/
        this.hand = hand;
        this.handTracker = handTracker;
        this.buttonDevice = mouse;
        this.viewPlatform = viewPlatform;
        this.worldBranch = worldBranch;
        
        /*Set up defaults and initalise*/
        viewTransform = new Transform3D();
        handTransform = new Transform3D();
        pickedObjectOffset = new Vector3f();
        pickedObjectRotation = new Matrix3d();
        draggingObject = null;
        
        /*Set up the behaviour conditions*/
        collision = new WakeupOnCollisionEntry(hand, WakeupOnCollisionEntry.USE_GEOMETRY);
        collisionExit = new WakeupOnCollisionExit(hand, WakeupOnCollisionEntry.USE_GEOMETRY);
        collisionMovement = new WakeupOnCollisionMovement(hand, WakeupOnCollisionEntry.USE_GEOMETRY);
        nextFrame = new WakeupOnElapsedFrames(0);
        wakeupConditions = new WakeupOr(new WakeupCriterion[]{nextFrame, collision, collisionExit, collisionMovement});    
        
        
    }
   
    public void initialize () {
    	wakeupOn (nextFrame);
    }
    
    //****************************************************//
    //                AVATAR MOVEMENT CODE                //
    //****************************************************//
    
	/* moves the world by the distance specified in the parameter vector */
	private void moveViewVector(Vector3f movement){
		
		/*Find the view direction*/
		viewPlatform.getTransform(viewTransform);
		Vector3f viewDirection = new Vector3f(movement);
		viewTransform.transform(viewDirection);

		/*Calculate the new transformation offset, the oppsite direction of the viewing angle*/
		Transform3D offset = new Transform3D();
		offset.set(viewDirection);
		
		/*Add the offset to the worlds existing location by adding to the appropriate translation parts of the matrix.*/
		Transform3D worldTF = new Transform3D();
		worldBranch.getTransform(worldTF);
		
		Matrix4d wtm = new Matrix4d();
		worldTF.get(wtm);
		
		wtm.m03 += viewDirection.x;
		wtm.m23 += viewDirection.z;
		if (SixDOF) /*This makes flying around the world a little easier by removing the up/down transformation*/
			wtm.m13 += viewDirection.y;
		
		worldTF.set(wtm);
		worldTF.get(currentWorldOffset);
		worldBranch.setTransform(worldTF);

   	}

	protected void forward(float distance){
		moveViewVector(new Vector3f(0, 0, distance));
	}
	
	protected void strafe(float inc){
   		moveViewVector(new Vector3f(inc, 0, 0));
	}

    private void moveVertical(float distance) {

		Transform3D viewTransform = new Transform3D ();
		viewPlatform.getTransform(viewTransform);
	
		Vector3f translation = new Vector3f();
		viewTransform.get(translation);
	
	    translation.y += distance;
	
		viewTransform.setTranslation(translation);
		viewPlatform.setTransform(viewTransform);
    }
	
    //****************************************************//
    //              HAND and OBJECT DRAGGING              //
    //****************************************************//
	protected void setHandPosition() {

		/*Set the locaiton of the users hand*/
		if (handTracker != null){
			/* get the hand location from the tracker */
			handTracker.getRead(handTransform);            
	        hand.setTransform(handTransform);
		} else {
			/* else set the hand location to directly infront of the user */
			viewPlatform.getTransform(handTransform);
			Vector3f viewDir = new Vector3f(0, -0.5f, -1.75f);
			handTransform.transform(viewDir);
			handTransform.setTranslation(viewDir);
			hand.setTransform(handTransform);
		}
		
        /*If we need to move an object*/
        if (draggingObject != null){
        	Vector3f viewDirection = new Vector3f(0f, 0f, -1f);
            viewPlatform.getTransform(viewTransform);
            viewTransform.transform(viewDirection);     
            moveDraggedObject(draggingObject, handTransform);
        }
        
	}
   
	/** tests to see whether the user is attempting to grab a new object with
	 * their hand. */
	protected void testForGrab(){
		
		 /*Make sure we can only drag one thing around at a time*/
		 if (draggingObject != null)
            return;
      
		 /*Is there is no object to drag? */
         TransformGroup group = getPickedObjectFromCollision();
         if (group == null)
            return;

         previousWorldOffset.set(currentWorldOffset);
       
         /*Chsnge the objects location to the same as the hands location*/
         Transform3D grouptransform = new Transform3D();
         group.getTransform(grouptransform);
         Vector3f groupvec = new Vector3f();
         grouptransform.get(groupvec);
      
         Transform3D handtransform = new Transform3D();
         hand.getTransform(handtransform);
         Vector3f handvec = new Vector3f();
         handtransform.get(handvec);         
         
         pickedObjectOffset.sub(handvec, groupvec);
         grouptransform.get(pickedObjectRotation);
      
         Vector3f viewDir = new Vector3f(0f, 0f, -1f);
         viewPlatform.getTransform(viewTransform);
         viewTransform.transform(viewDir);
         
         draggingObject = group;
         moveDraggedObject(draggingObject, handTransform);
      
      }
   
	/*This method finds the TransformGroup of the object being dragged around the world*/
	public TransformGroup getPickedObjectFromCollision() {
		
		SceneGraphPath draggedObject_;
		
        if (collision.hasTriggered())
            draggedObject_ = collision.getTriggeringPath();
        else if (collisionExit.hasTriggered())
        	draggedObject_ = collisionExit.getTriggeringPath();
        else if (collisionMovement.hasTriggered())
         	draggedObject_ = collisionMovement.getTriggeringPath();
        else
            return null;
      
        Node node = draggedObject_.getObject();
        if (node == null) 
            return null;
      
        Object userData = node.getUserData();
        if (userData == null) 
            return null;
      
        if (!(userData instanceof TransformGroup))
        	return null;
      
        /*If we find an object that can be dragged around the world return its transform group*/
        return (TransformGroup)userData;
	}
	
	/* moves a dragged object relative to the hand movement. 
	 * This method will move an object being dragged relative to the user's
	 * hand. Because the hands remain stationary while moving, we compute the
	 * object's new location as:
	 * 	    handOffset - pickedOffset + movementOffset
	 * Where:
	 *  - handOffset is the the user's hand's offset from 0, 0, 0
	 *  - pickedOffset is the vector representing the point on the object where
	 *    the object was picked
	 *  - movementOffset is the offset the user has moved since picking up the object
	 * */
	protected void moveDraggedObject (TransformGroup object, Transform3D hand) {
		Transform3D objectTransform = new Transform3D(hand);
        Vector3f objectTranslation = new Vector3f();
        objectTransform.get(objectTranslation);
        objectTranslation.sub(pickedObjectOffset);
        objectTransform.setTranslation(objectTranslation);
        objectTransform.setRotation(pickedObjectRotation);
        
        /* calculate the movement offest by comparing our previous
         * world offset with the current world offset. If they're different,
         * the user has moved */
        if (currentWorldOffset.x != previousWorldOffset.x ||
    		currentWorldOffset.y != previousWorldOffset.y ||
    		currentWorldOffset.z != previousWorldOffset.z){
        	Vector3d v = new Vector3d();
        	v.sub(currentWorldOffset, previousWorldOffset);
        	movementOffset.add(v);
        	previousWorldOffset.set(currentWorldOffset);
        }
        	
    	Matrix4d fly = new Matrix4d();
    	objectTransform.get(fly);
    	
    	fly.m03 -= movementOffset.x;
    	fly.m23 -= movementOffset.z;
    	if(SixDOF)
    		fly.m13 -= movementOffset.y;
    	objectTransform.set(fly);        
        object.setTransform(objectTransform);
	}
   
	/* if the user is dragging an object, drop it */
	public void testForDrop() {
		if (draggingObject != null){
			draggingObject = null;
			movementOffset.set(0,0,0);
		}
    }
}
